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ABSTRACT 


A six degree- of-freedom computer -controlled manipulator is examined, 
and the relationships between the arm's joint variables and 3- space are derived. 
Arm trajectories using sequences of third-degree polynomials to describe the 
time history of each joint variable are presented and two approaches to the 
avoidance of obstacles are given. The equations of motion for the arm are 
derived and then decomposed into time-dependent factors and time-independent 
coefficients. Several new and simplifying relationships among the coefficients 
are proven. Two sample trajectories are analyzed in detail for purposes of 
determining the most important contributions to total force in order that 
relatively simple approximations to the equations of motion can be used. 
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I. INTRODUCTION 


The Jet Propulsion Laboratory is engaged in a robot research program 
("Artificial Intelligence for Integrated Robot Systems") aimed at the develop- 
ment and demonstration of the ability to integrate a variety of robotic functions. 
These functions include locomotion, manipulation, sensation, perception, and 
decision making. The immediate objective of the program is the building of a 
robot breadboard which demonstrates the integration of these capabilities and 
performs autonomous goal-directed coordination of the separate functions. 

Broad goals are provided by man. 

The robot breadboard software is in two computers, a local mini- 
computer and a remote larger computer. The minicomputer is generally used 
as a real-time controller (RTC), being sequentially dedicated to the control of 
one of the functional subsystems (vehicle, manipulator, etc). The RTC is in 
direct communication with the breadboard hardware. The larger remote 
computer, generally considered as a timeshared computer (TSC) not dedicated 
to any one task, is used primarily for planning purposes. This report con- 
siders the functions performed by the manipulator software residing in the TSC. 
Manipulator kinematics, trajectory generation, obstacle avoidance, dynamics, 
and TSC software are discussed. The manipulator under consideration is a 
JPL-built modification of the Stanford Electric Arm (Refs. 1, 2), a six degree- 
of-freedom arm designed for computer control. 
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II. MANIPU LATOR KINEM AT ICS 


Manipulator kinematics have been analyzed and presented elsewhere 
(Refs. 3-9) and are summarized here only for clarity. This section, then, 
briefly describes general manipulator coordinate system conventions, applies 
these to the JPL manipulator, and presents the relations between joint -variable 
space and arm position/orientation. 

A. Coordinate System Conventions 

Consider a system of n mechanical links (numbered 1 through n), each 
capable of linear or rotary motion relative to the adjacent links. For 
reference, define link 0 to be fixed to a base (in a rigid table, on a vehicle, or 
on the ground, for instance). Define a joint to be the intersection of two 
adjacent links; in particular, joint i is the intersection of links i- 1 and i, 
i = 1, . . . , n. 

Then (see Figs. 1, 2) define the following: 

= axis of joint i+1 (sense arbitrary). If the joint is rotary, then 
is the axis of rotation; if prismatic (i. e. , linear), then is in the 
direction of linear motion. Defining z n in an arbitrary manner, z- 
is defined for i = 0, . . . , n. 

X| = Zj_ j x ^i^||^i-l x Zi || * the common normal between and z^, 

directed from the former to the latter. If X z. = 0, then x- 

is arbitrary subject only to • Zj = 0. Defining xq in an arbi- 
trary manner (with Xq» = 0), is defined for i = 0, . . . , n. 

yj = z i X x^, i = 0, . . . , n. 

Thus an orthonormal coordinate system (x^, y\, af. ), fixed in link i, i = 0, . . . n, 
is defined. The following parameters define the relationship between succes- 
sive coordinate systems: 

0. = angle from x. ^ to 5T., measured positively counterclockwise about ^ 

r. = distance along IT. , from x. , to x.. 
l ® l-l l-l i 

s angle fromT , to "z., measured positively counterclockwise about 3c.. 

l i-l l i 

a. = distance along "x. from 1. , to“z.. 
l ® i l- 1 i 
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A vector v* expressed in system i can be expressed in system i-1 as 
using the transformation as follows: 


More specifically, v* is first rotated about x* by - {aligning and z^j), 
then translated along by a^ (bringing and z^_ j into coincidence), then 
translated along z^ (^z^j) by ^(giving x^ and x ^_2 a common origin), and 
finally rotated about z^ by - 0^ {bringing the two systems into coincidence). 


AXIS axis 

OF OF 

JOINT JOINT 

i i+i 




Fig, 1, Standard Manipulator Parameters 
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where s ~ sin (x) and c = cosfx}. 
x x 

Since the axes of motion are z^ {by definition), then and a ^ are constant 
for all i; either $. (if the joint is linear) or r. (if rotary) is constant for 
each i. 

The upper left 3x5 partition of t|_j expresses the rotation of frame i 
relative to frame i-1. The upper right 3 X 1 partition expresses the 
position uf tlie origin of frame i relative to i-1. 

B, The JPL Manipulator 

The JPL arm (Fig. 2) is characterized by the constant parameters 
a-, (i ^ 3) and 6 ^ given in Table 1. All joints except the third are 

rotary. The joint variables 8 ^ (i ^ 3) and r 3 form the joint-variable vector 
8 given by 

8 — 8 T-y 8y 8^> 
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Table 1. The JPL Manipulator 


i 

a i 

(deg) 

r i 

(cm) 

(deg) 

a- 

X 

(cm) 

Maximum 
radial dimension 
about z ^ 

(cm) 

Maximum linear 
extension along 

- z i 

(cm) 

1 

-90 

35. 56 

-175, 175] 

0 


6. 67 


0 

2 

90 

16. 19 

-175, 175] 

0 


6. 99 


30. 48 

3 

°l 

14. 0, 111. 8 

-90 

0 


3. 18 


139. 70 - r 3 

4 

-90 

0 

-175, 175] 

0 




0 

5 

90 

0 

1 - 110 , 110 ] 

0 

■ 

6. 03 


0 

6 

0 

24. 77 

[-175, 175] 

0 


> 


0 

fingers 

12. 7 

cm long, : 

5. 02 cm wide, open to 

5. 72 cm each 


Note: The ranges of the six joint variables are indicated in brackets above. 

— 


The vectors x^ and the i = 0, ...» 6, are defined as in Fig. 2. It is 
convenient to define four additional vectors (Fig. 3): 


the approach vector a = z^, pointing in the direction of approach of 

the hand; 

the sliding vector "s = y , » pointing in the direction of finger motion 

o 

as the hand opens and closes; 

the normal vector n = x^, orthogonal to the plane of the fingers; 
and the position vector p, pointing from the origin of the base system 
to the origin of the hand system (n, 's,‘a). 


Applying the arm parameters of Table 1 to the transformations T 
defined in Eq„ (1) yields the six transformations t! j, i = 1, ... 
in Appendix 1. The arm transformation T is given by 


T ~ T 0 = T J T 1 T 2 T 3 T 4 T 5- 


i 

i-1 

6, given 
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HAND COORDINATE FRAME 



Fig. 3. Hand Position and Orientation 
Expressed in Base Coordinates 

The columns of T are respectively, the normal, sliding, approach, and posi- 
tion vectors defined above. T is expressed in full in Appendix 1. 

The arm is mounted on the vehicle as in Fig. 4 such that z is directed 
upwards from the vehicle orthogonal to the plane of the vehicle platform 
and points ahead in the direction the vehicle is aimed, is to the 
right. * 

The origin of this system is the point on the vehicle platform at the 
center of the post comprising link 1. 

Note that z q is not in general colinear with the gravity vector. The vehicle 
platform may not be gravitationally horizontal. 


❖ Where otherwise unspecified, the terms "right' 1 and "left" are used with 
respect to an observer standing on his feet facing in the direction of y . 
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The physical construction of the arm and the potentiometers permits 
only 350 deg of rotation for joints 1, 2, 4, and 6 and 220 deg for 
joint 5. The 10 deg (140 deg for joint 5} of deadband can be placed 
anywhere and are specified below and in Table 1. The joint variable r^ 
is constrained by hardware to motion within the limits given in Table 1. 

The first joint variable, 0^, varies from -175 deg to +175 deg. 0 ^ is 
0 deg when (the boom side of) the shoulder (i. e. , the link 3 side of link 2) 
is pointed straight ahead (i. e. , in the direction of y^}. 0 ^ = +90 deg or 

-90 deg puts the shoulder to the left or right, respectively. The shoulder 
cannot point closer than 5 deg to the rear of the vehicle. 

Coordinate system 1 (*1 , y , ) is located at the intersection of the cen- 

ters of the post and shoulder. 0^ varies also from -175 deg to +175 deg 
and is 0 deg when the hand side of the boom is directed straight up (i.e. , 
along 2 ^ ). When 0^ > 0 the arm looks like a human left arm; when 0^ < 0, 
the arm is a right arm. Where feasible, the arm will not "flip" (over the 
top) from right to left or left to right. There will, however, be occasions 



Fig. 4. Arm on Vehicle Base and Hand Coordinates 
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on which the flip motion is desirable if not mandatory. The deadband for 
0^ is straight, down. 

Coordinate system 2 is located at the intersection of the cen- 

ters of the shoulder and boom. 

Coordinate systems 3, 4, and 5 are all located at the common intersection 

of z\, z At and $ varies from -175 deg to +175 deg and, when $ is 

0 deg and 0 ^ is -90 deg (as in Fig. 3), the axis z^ of joint 5 is up (along 

z“ ), The deadband of joint 4 is thus in the opposite direction. Joint 5 

varies only from -110 deg to +110 deg in order that the hand not crash 

into the boom. 0 r is 0 deg when the wrist is directed as an extension 
5 

of the boom. The deadband is where the wrist is directed back up the 
boom. 

0 varies from -175 deg to +175 deg and, when 0 deg, the sliding axis is 
6 

along the axis 'z of joint 5 (as in Fig. 3). 

C. Kinematic Solutions 

Given a joint-variable vector 0, the position/orientation of the terminal 
effector is described by 


T = T(6)=Tq = 

There are four solutions to the problem of finding the joint-variable vec- 
tor 0 yielding a given position/orientation T, two for a right-arm configuration 
(0 < 0) and two for a left-arm configuration (0 > 0). For each configuration 

there is one solution with 0^ positive and another with 0^ negative. 

Briefly, the solutions are found by the following procedure. (For details 
see Appendix 2a.) Define a vector 


n s a p 

0 0 0 1 


' d ! ' 


_ 0 ' 


0 

d 2 

= P “ r 6 a - 

d„ 


r , 

3 

L 
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the shoulder-to-end-of-boom vector. The boom extension length is found by- 
noting that || d II = r 2 + r 3 * The joint 2 cosine is given by d^/r^, giving 
rise to the two possible values for 0^. The value of 0^ is computed from 
the first two components of d as 


= tan 


-1 


d 2 r 3 S 2 ~ d l r 2 
d l r 3 S 2 + d 2 r 2 


Thus the first three joint variables are known, giving the base system 
3 3 

boom transformation Tq. Considering T^ to be of the form 


x 3 y 3 Z 3 P 3 


0 0 0 1 


and defining a reference vector z as the unit vector in the direction X a, 
the remaining joint variables are found to be 


0 . = tan 
4 


-1 


y_ X z • z. 


y- .z 


0- = tan 
5 


-1 


z^ X a « z 


z ^ • a 


, and 


9^ = tan 


-1 


z X s • a 


z • s 


If ( 0^> 0^, 0^) are solution values, then so also are 

( 0 4 + 7T » " e 5 > e 6 + *). 

For very slight hand position/ orientation changes the rates of change of 
joint variable with respect to position/ orientation change may be computed. 
The details of arriving at the required incremental changes in the joint vari- 
ables are described in Appendix 2b. 
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III. TRAJECTORIES AND OBSTACLES 


In this section various constraints on moving the manipulator from one 
position/ orientation to another are compared with the objectives of such motion 
to yield the currently implemented trajectory form. The term "trajectory” is 
here meant to refer to the partial or complete description of the path that the 
arm follows. 

A. General Considerations 

Four constraints are imposed on the form of the planned trajectory. 

First, continuity of joint variable (position) and its first two time derivatives 
(velocity, acceleration) must be guaranteed. Second, the trajectory must be 
readily calculable in a noniterative manner. Third, extraneous motion must 
be minimized. Finally, it must be possible (and straightforward) to determine 
and specify intermediate conditions (i. e. positions); collision detection and 
avoidance and possibly other functions cannot otherwise be served determinis- 
tically, (See below. ) 

The trajectory form may vary along a deterministic/conditional con- 
tinuum, depending on the degree to which it is to respond to external sensory 
data during trajectory execution. At one extreme is a purely deterministic 
trajectory, planned completely in advance of execution. Only a catastrophe 
halts execution of the planned path. In this case, any external sensing to be 
done is performed during the planning stage. Internal sensing (of joint- 
variable state) is used throughout execution to maintain the adherence of actual 
motion to plan. Deterministic planning assumes a static world during arm 
motion as well as sufficient a priori knowledge and execution accuracy; adap- 
tive control is limited. 

Towards the other extreme is conditional planning, the nature of which is 
highly dependent on the specific external sensors used. Conditional planning 
brings response flexibility and possible planning time reduction at possible 
increased real-time computation requirement costs. It is currently intended 
that a conditional overlay to a deterministic trajectory plan be implemented. 
Since the nature, number, and placement of rapid response external sensors 
for the JPL robot is as yet unspecified in sufficient detail, only the determin- 
istic trajectory is described below. 
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A related distinction concerns the manner in which the trajectory plan is 
specified. Either a sequence of two or more points can be selected ("point-to- 
point M trajectory), or the complete time history of the path ("path 11 trajectory) 
can be specified. In the obstacle -rich environment in which the JPL manipu- 
lator is to work, path planning must be selected in order to be able to effec- 
tively detect collisions in the plan. 

Path planning can be conducted in either joint -variable space or in 
3-space. In the former case, the time history of each joint is planned; it is the 
combination of the time histories of the joint variables that describe the motion 
of the arm. In the latter case, it is the motion of a particular point on the 
manipulator (commonly a point on the hand) that is planned; the required joint 
variable time histories are derived from the plan. Planning in joint variable 
space is implemented. The advantage of this selection is that the plan is 
formulated more directly in terms of the variables to be controlled during 
motion. The associated disadvantage is the difficulty in determining where 
the various links will be during motion, a task required to guarantee obstacle 
avoidance. 

B. Polynomial Trajectories 

In both the JPL Robot Research Program and the Stanford Hand-Eye 
Project (Ref. 6), the emphasis to date has been on deterministic path planning 
in joint variable space. Specifically, the time histories of the joint variables 
are specified as polynomial sequences. Continuity of joint variable position, 
velocity and acceleration can be guaranteed, intermediate positions can easily 
be specified and determined, the trajectories are readily calculable noniter - 
atively, and it is possible to reduce extraneous motion to negligible amounts 
using polynomial trajectories. 

If the trajectory for a given joint uses p polynomials, then 3(p + 1) 
coefficients are required to specify initial and terminal conditions (position, 
velocity, acceleration) and guarantee continuity of these variables at polynom- 
ial boundaries. An additional coefficient is required for each intermediate 
condition (position, for example) specified. Generally two intermediate positions 
are specified, one near departure and one near arrival, in order that safe depar- 
ture and approach directions can be more directly controlled. Thus, one 


12 


JPL Technical Memorandum 33-679 



seventh degree polynomial for each joint would suffice; so also would 
two cubic s and a quintic (3, 5, 3), two quartics and a cubic (4, 3, 4), or 
five cubics. Only the last three sequences are considered here. 

Fig. 5a shows the time history of a joint variable described by a cubic, 
a quintic, and another cubic. It has been found that trajectories using polynom- 
ials of degree five or higher typically wander, as shown. This behavior 
appears in observation as gross extraneous motion of the arm. 

For single-segment trajectories (those for which no intermediate posi- 
tions are specified), the quintic illustrated in Fig. 5b provides motion without 
wandering. This quintic has been presented in Ref. 6 and applies to incremen- 
tal motion of joints. (This "incremental quintic" is used in conjunction with the 
incremental joint variable solutions of Appendix 2b). 

Use of a quartic-cubic-quartic trajectory reveals a somewhat different 
problem. As shown in Fig. 5c, the desire to assure an appropriate direction 
of departure and approach of the terminal effector can be thwarted by the ten- 
dency of the quartic-cubic-quartic trajectory to overshoot or undershoot its 
endpoint values. 

A third polynomial sequence, five cubics, is being implemented for the 
JPL arm. This trajectory appears to minimize the "wander" and "overshoot" 
problems. Typically, as in Fig. 5d, there is no overshoot; wander, when it 
occurs, is small. 

The five cubic trajectory permits easy modification for obstacle avoid- 
ance, as does the (4, 3, 4) trajectory. In avoiding an obstacle quite often a 
single intermediate position for a joint will be added. This necessitates the 
addition of only a single cubic with its four coefficients; one coefficient is used 
to satisfy the new condition and the other three maintain continuity of 0, v, 
and a. 

The determination of the relative times for the three main trajectory 
segments is accomplished initially by a heuristic. After modifying the trajec- 
tories, however, the desired maximum joint accelerations may be exceeded. 

By evaluating the accelerations for each trajectory segment for each joint at 
their extreme (the end points, since acceleration, the second derivative of a 
cubic, is linear), the maximum acceleration relative to the limit of 
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a) Sequence of Cubic-Quintic- Cubic Polynominal Segments 
(Showing 11 Wandering") 



b) The "Incremental Quintic" 




c) Sequence of Quartic- Cubic -Quartic d) Sequence of Five Cubic 
Polynominal Segments Polynominal Segments 

(Showing "Overshoot") 

Figure 5. Various Polynominal Joint Trajectories 
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acceleration can be determined and then all time intervals t. scaled up propor- 
tionate to the square root of the excess acceleration rates. This guarantees 
observation of the acceleration limits and also obviates the need to recompute 
polynomial coefficients. On all the polynomial trajectories considered above 
the coefficients remain constant under changes in the h if the ratios h/t. 
remain constant for all i and j; multiplying all times by a constant leaves all 
polynomial coefficients unchanged. 

C. Obstacle Classification, Detection, and Avoidance 

In this section, the problems of classifying, detecting, and avoiding 
obstacles are discussed. Two methods of dealing with obstacles permanently 
fixed to the vehicle is presented, and alternative strategies for avoiding 
obstacles in the environment are also described. 

1. Classification 

The purpose of classifying obstacles is to form an exhaustive list of 
obstacles in order that all collisions can be considered in assuring the safety of 
planned arm motion. One possible taxonomic representation of disastrous 
occurrences to be avoided is presented here and summarized in Table 2. 

The manipulator itself is an obstacle. The hand (link 6) is capable of 
colliding with the portion of the shoulder (link 2) extending away from the boom 
(link 3). The hand can also hit the support post (link 1} or the arm mounting 
base (see Fig. 6). Furthermore, each joint is limited in its motion. These 
joint stops are considered here as obstacles, for one way of avoiding commands 
that violate these stops is also a feasible way of dealing with other obstacles. 
Since these manipulator internal obstacles are always present, they are con- 
sidered here as a type of permanent obstacle* 

Vehicular obstacles, consisting of all vehicle mounted hardware capable 
of colliding with the arm (see Fig. 7), are also permanent. The TV/laser sys- 
tem, the vehicle platform surface and edges, the wheels, and the wheel drive 
motors comprise the set of permanent vehicle obstacles. 

All of these but the platform itself move with respect to the manipu- 
lator base coordinate system (x Q , y Q , T Q ). The volumes occupied by the 
TV/laser, wheels, and motors can be based on either an assumed standard 
position, calculated using the actual steering angle and TV/laser pan and tilt. 
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Table 2. Obstacles and Colliders 


OBSTACLES 

1. Internal (permanent) 

1. 1 Joint stops 

1. 2 Link 1 

1. 2. 1 Support post 

1. 2. 2 Base 

1.3 Link 2 (shoulder) 

2. Vehicular (permanent) 

2. 1 TV/Laser ( envelope , assumed position, or vari- 

able position) and support 

2,2 Wheels ( assumed position , envelope, or variable 
position ) 

2. 3 Wheel motors (assumed position, envelope, or 
variable position) 

2. 4 Platform 

2, 5 Frame edge 

3. Environmental (nonpermanent) 

3. 1 Placed on vehicle 

3. 2 In natural state 

4. Two Arms (nonpermanent) 

COLLIDERS 

Link 3 (boom) 

Boom front {= hand rear) 

Boom rear 
Boom edge 

Link 6 (hand) 

Hand front 
Hand side 
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LINK 1 (POST) U.2.1) 


Figure 6. Some Internal Obstacles 
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or an envelope of all possible positions can be used. In the case of the wheels 
and motors, it is assumed that the steering angle is always zero, that is, that 
the wheels point straight ahead such that the axis of wheel rotation is always 
along the manipulator base frame axis x Q . Since the manipulator and the 
vehicle are not driven simultaneously, and since the vehicle wheels can be 
turned while the vehicle itself remains stationary, this constraint does not 
appear to be overly restrictive. In the event that this assumption proves to be 
too restrictive, then an envelope of all possible wheel/motor positions will be 
used. The envelope is only slightly larger than the volume consumed by a single 
position both because the area of inside wheel surface above the platform is 
small (the wheel axis being four inches below the platform) and because the 
steering angle is limited (perhaps to ±30 deg). Calculating the location of the 
occupied volume is considered undesirable and a nuisance; one of the purposes 
of categorizing some obstacles as permanent is to avoid numerous computations 
and to take advantage of simpler invariant collision checks that can be devised 
for permanent obstacles. 

An envelope of all possible TV/laser positions has been selected for 
implementation over an assumed position because of the wide variability in the 
location of the occupied volume as the TY/laser head pans and tilts. 

Environmental obstacles include items placed on the platform by the man- 
ipulator as well as such features as rocks, ground slopes, and surface holes. 

If and when the robot breadboard becomes equipped with two manipulators 
that work together, then each manipulator becomes a nonpermanent (in fact, 
moving) obstacle to the other. 

2. Detection of Permanent Obstacles 

Joint limit violations are detected by evaluating each trajectory polynom- 
ial at its extreme values and comparing the extrema against the acceptable 
limits. Finding the extrema is particularly straightforward with cubics, as 
they are either at the endpoints or at the roots of the quadratic derivative of 
the trajectory cubic. 

All other permanent obstacles are detected by evaluating the trajectories 
for the six joints at numerous (currently 21) points along the trajectory and 
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comparing the resulting sequence of link positions with the positions of the 
fixed obstacles. 

Generally, collision testing of this type involves determining the distance 
between a link (i. e. a line with a link- radius and safety- radius added, forming 
a cylinder) and a point, line, or combination of lines (obstacle envelope edges). 
Computation is reduced considerably by the invariant geometry of the obstacles; 
numerous simple tests based on various joint variable values can be performed. 
In many cases, permanent obstacle detection is reduced to the task of evalu- 
ating certain simple inequalities. 

Link equations and link distance formulas are given in Appendix 4. 

3. Avoidance of Permanent Obstacles 

Two general methods of obstacle avoidance are presented here* each of 
which is applicable to both permanent and nonpermanent (environmental) 
obstacles. The two approaches are referred to as "the freeway method" and 
"adding cubics". 

The freeway method consists of the precomputation and storage in a data 
file of a number of trajectories relating commonly accessed arm positions/ 
orientations. Various park positions, tool bin and rock box locations on the 
platform, manipulator positions and position sequences for presenting objects 
to the sensory subsystem, and perhaps some target locations on the ground are 
all commonly accessed points that can be related to each other by precomputed 

trajectories, i. e. , freeways. Slight variations from these standard initial and 
terminal points can be dealt with using freeway entrance and exit trajectories. 

The freeway method saves computer time and guarantees a safe path at 
the cost of more storage and somewhat reduced flexibility. Flexibility is 
retained using the addition of cubics method; with both implemented, the freeway 
method becomes a valuable adjunct. 

Adding cubics is the other implemented obstacle avoidance procedure. 
Specifying an additional intermediate condition for a joint (position, normally, 
although other intermediate conditions can be used) and an additional trajectory 
segment time, one can add a cubic such that the resulting trajectory satisfies 
all the original constraints as well as the new one. By partitioning an original 
trajectory segment time, the trajectories and the times for the other joints and 
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for the preceding and terminal segments of the modified-trajectory joint need 
not be changed; hence only a few coefficients require recomputation. Adding a 
cubic is achieved by repeated application of the relationships in Table 4 of 
Appendix 3. 

In the planning of obstacle avoidance maneuvers, either planning control 
or execution control of the trajectory can be implemented. Planning control 
assumes poor accuracy during execution time; obstacles are avoided by using 
large safety margins and leaving room for large errors. In execution control, 
it is assumed that fairly high positional accuracy is achieved in executing a tra- 
jectory and thus only small safety margins are required. For the JPL- manipu- 
lator, it is expected that no point on any link will ever be more than 1.2 7 cm 
from where it was planned to be and, in general, will be much closer than that. 
Thus very small safety margins are used. Details of detection and avoidance 
of specific permanent obstacle are provided in a series of JPL internal 
documents . 

4. Detection and Avoidance of Nonpermanent Obstacles 

Nonpermanent obstacles can be detected and avoided in the same general 
ways as indicated above for permanent obstacles. Furthermore, careful 
selection of approach direction and final hand orientation can be of use. There 
are two additional considerations in avoidance. First, it can be assumed that, 
in the real world, out of doors, obstacles rest on the ground and hence can be 
avoided by going over them. Second, a type of reflexive obstacle avoidance 
(applicable as well to the task of aiding the coordinated motion of two arms) is 
possible using sensors of the type described in Ref. 10. 
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IV. DYNAMICS 


In this section, some dynamic considerations impacting planning program 
implementation are presented. The equations of motion of the arm are given 
(based on Ref. 5) and kinematic -dependent coefficient matrices are separated 
from time -dependent factors in the equations of motion (as in Ref. 6) in order 
to derive several computation- reducing relationships. Sample trajectories are 

used to compare the effects on computed generalized force of several 
approximations. Finally, a brief discussion of forces and moments with an 

example application to the task of weighing objects is given. 

A. Equations of Motion and Coefficient Matrices 

The equations of motion for the manipulator can be expressed (See 
Appendix 5) as 


6 6 6 

F i = c i + 2 c ik\ + £ £ c ik m ^k^ m - 6 - 

k-l k=l m = 1 

where F. is the generalized force (force for joint 3, torque for the other five 
i th. 

joints) applied to joint i, q^ is the* i joint variable (r^ for joint 3, 6^ for the 
other joints), and the C’s are the time -independent (but joint position depend- 
ent) coefficients defined in Appendix 5. 

The first term, C^, describes the effect of gravity on link i. The sec- 
ond set of terms describes the reaction force on link i of the acceleration of 
link k (if k^i) or the force required to accelerate the link and overcome motor 
(i. e. rotating) inertia (if k=i). The third set of terms describes the centrifugal 
(k=m) and Coriolis (k^m) analogs for the arm, the effects on link i of the 
coupled velocities of links k and m. 

Only 83 of the 258 coefficients need be computed; the rest are determined 
from the following (proven in Appendix 6): 


1. 

ii 

•H 

O 

C ki 

2. 

C ikm 

= C 

3. 

C i3m 

= 0, 
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4. 


i, m ■> k 


iki 


0, i > k, and C 


^ikm ^mki* 


kkk 


°] 


5. 


0 if the arm-relative gravity vector is vertical. 


The first statement indicates that the reaction force coefficient for link i 
due to the acceleration of link k is the same as the coefficient for link k due to 
the acceleration of link i. 


The second statement follows directly from the equations of motion. 

The third statement indicates the lack of interactive effect on any joint of 
the boom (link 3) velocity and the three wrist link velocities. 

The fourth statement demonstrates the reflexive interaction of outer links 1 
(i and m) velocities as they couple with the velocity of an inner link k. Also, 
there is no interactive effect on any outer link due to coupling of its own veloc- 
ity with that of any inner link, and, in addition, no joint feels any of the 
radially-directed centrifugal effects of its own motion. 

The fifth statement indicates that joint 1 feels no gravity effects when the 
first link is vertical. 

Each time it is desired to determine the nominal applied force/torque F^, 
then only 83 joint position dependent coefficients need be computed. It is useful 
to know how many times these coefficients need be recomputed over a whole 
trajectory. 

B. Generalized Force Over Trajectories 

Consider the two sample trajectories described in Fig. 8. In the first, 
the arm moves from a left arm park position with extended hand and horizontal 
boom up 7. 5 cm; then down to a spot 7. 5 cm above the ground and 56 cm in 
front of the vehicle and finally down to the ground. The second trajectory 
moves the arm from the latter position to a point on the vehicle platform 89 cm 
behind the arm post. Both trajectories are in three parts, one for liftoff, one 
for the major motion, and one for setdown, and have been computed with 
five cubic s. 

The time histories of the generalized force vector F over the two 
trajectories have been plotted in Figs. 9 and 10. For each joint and for each 
trajectory four curves are plotted, one each showing the contribution of the 
gravity term (the curves labelled "G"), the velocity terms ("V 1 '), the accelera- 
tion terms ("A") and the total force or torque ("F"). 
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Joint 1 feels no gravity effects (vehicle assumed level). Almost all of the 
total torque required for the joint for the two sample trajectories is due to 
the acceleration terms; the velocity term are only significant briefly. Joints 4 
and 6 also display this tendency. In fact, the inertial terms (i. e. , those of 
the form C . . q . ) clearly dominate the off-diagonal reaction acceleration terms, 
especially when the joint in question is being driven. (Recall that the diagonal 
coefficients C.. include the very large motor inertias. ) 

In both trajectories, joint 2 also experiences high acceleration terms 
relative to total torque, with moderate contributions from the velocity terms. 
However, for joint 2, gravity is often significant and tends to separate the 
acceleration torque and total torque and total torque curves. 

Joints 3 and 5 frequently experience large contributions from all three 
sources, as shown. 

While it is true that of the 258 coefficients, only 82 or 83 need be com- 
puted, the que stions remain as to how frequently they should be recomputed 
and how many of them (and which ones) should be recomputed. It is beyond 
the resources of the realtime computer to recompute all 83 60 times each 
second while still performing other operations. 

Figures 11 and 12 begin to address these questions. In these figures, the 
curves labeled I represent the errors arising from interpolating the coefficients 
between the interpolation points indicated on the time scale. These inter- 
polation points are at the beginning and end of the trajectory departure and 

arrival segments. If F is the fully computed force/torque ( = F (C., C , C ... ) ) 
A A A A A 1 1 J K 

and F is the interpolated force/torque curve (F = F (C., Ch ., C.^)), then the 

curve labeled I is given by F - F. Thus the correct torque F to apply, 

A 

F = F 4- I, 

is comprised of a nominal torque F (coming from the plan) and an error tor- 
que I (coming from the control system). 

The curves labeled II describe the interpolation error when all velocity 
terms are ignored. The curves labeled III only consider gravity and inertial 
acceleration terms, interpolated. The latter two interpolations have been used 
successfully in the Stanford Hand-Eye Project, the occasionally large errors 
for these approximations notwithstanding. 
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Fig. 12. Trajectory 2 Interpolation Errors 
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These trajectories are extremely rapid, requiring only 1.70 and 1.05 sec- 
onds, respectively. Doubling the trajectory times (to 3.40 and 2. 10 seconds) 
cuts all components of all the curves of Figs. 9, 10, 11, and 12 by a factor of 4, 
with the exception of the gravity curves and gravity components. This two to 
three second trajectory time is more representative. Plots for interpola- 
tion III and full torque for joint 5, both trajectories, appear in Fig. 13 with 
original and doubled times compared. 

It is desired to reduce the approximation and interpolation errors both 
in absolute terms and relative to the actual desired torque F. Adding an addi- 
tional interpolation point in the middle of the mid-trajectory segment gives the 
error curves plotted in Figs. 14 and 15 (original time scale). Note that the 
error torques for joints 3 and 5 are still especially large. 


Most of the error remaining in the error curves comes from neglecting 
the centrifugal terms C.^q^ for j = 1, 2, 4, 5. (Recall that C..^ = 0. ) Adding 
these terms back yields the quite acceptable error curves IV plotted in Figs. 16 
and 17. For comparison, the full torque curves are plotted. 


Finally, in Figs. 18 and 19» these same curves are plotted with doubled 
times (two to three seconds), accurately reflecting currently planned trajec- 
tory times. The vertical scale in these figures indicates the nominal, 
error, and full torques relative to the limit for each joint. Observe that in 
neither trajectory does any joint come very close to the limit, and, in fact, 

the applied torque will rarely exceed 25% of the limiting values. Note further 

A 

that with the two to three second trajectory, the nominal torque F is generally 

A 

quite close to the desired torque; IV = F - F is small. Finally, it is noted 
that the computations required in implementing this approach are well suited to 
minimizing compute r - to - compute r communication buffer size and to an efficient 
allocation of activity between the planning computer and control computer. 

C. Forces and Moments 

Ref. 6 describes the effects of applying a force F^ = (f^, f , 0) 

and moment M n = (M ,M , M , 0) to the end of the hand. (F and M expressed 
0 x y z 

in base reference system, as indicated by the subscripts. ) In hand coordinates, 


F 6 = T 6 F 0 


and 


M 6 = T 6 M 0 
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JOINT 3 






The forces and moments felt at the other joints are given by 



T-! , F. 
J- 1 J 



, j = !> 


( 19 ) 



. M. + P; X F 4_! 
J-l J J J 1 


, j - 1, • • • > 6 1 


( 20 ) 


where 


P. = column 4 of .. 

} j- 1 

The reaction torque is the negative of the third component of if joint j is 
revolute; the reaction force is the third component of F^ if joint j is prismatic 
(i. e. , if j = 3). The third component is the one acting in the direction, the 
line of action for joint i + 1. The reaction vector consists of five reaction 
torques and one reaction force (for joint 3). 


Suppose a 1 kg rock is enclosed in the hand as the arm lifts and then 
stops. Assuming the vehicle to be gravitationally horizontal, we can compute 
the reaction vector R for fn ] 


0 


r 6^ s 2 C 5 + C 2 S 45 ) + r 3 s 2 


and Mq = 0 as R 


' C 2 

r 6 S 25 C 4 

r 6^ S 24 C 5 + C 2 S 5^ 


0 


The error torque set up when a rock is lifted can be compared to the above 
reaction vector R for a 1 kg rock to obtain the weight oi the rock. By keeping 
the arm unbraked and motionless, error torques that are free from the 
approximation effects of the previous section can be read. 
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V. SOFTWARE 


The manipulator software fits into the overall robot system software 
structure as indicated in Fig. 20, An operator provides goals to either the 
robot executive (REX) or the time-shared computer manipulator programs 
(TSCMP's). In either case, the communication is mediated by the Master 
Control Program (MCP), 

The Robot Executive consists of three parts: a cognitive executive, which 
plans activities by sequencing and combining the planning activities of individual 
robot subsystems (i. e. manipulator, vehicle, TV/laser rangefinder): a world 
model into which the resulting subsystem plans, state of the robot, and state of 
the world are stored; and an operating executive, responsible for execution of 
the planned activities. 

The TSCMPs can likewise be considered as being comprised of three 
program groups: a group of "up -interfacing " programs which serve to decode 
commands to the arm and sequence specific-motion planning activities; a group 
of planning programs which compute trajectories, torques, and other quantities 
(as partially described in the preceding sections); and a group of "down- 
interfacing" programs which code the results of planning program activity for 
the RTC. 

Operator or cognitive executive -generated plans start the TSCMP's. The 
packed buffers (see below) representing the plan are then either returned to the 
cognitive executive for naming and storage in the world model, or they are 
directly transmitted by the TSCMP's to the RTC. In the latter case, RTC 
feedback goes back to the TSCMP's and then, after processing, to REX. In the 
former case, the operating executive selects the appropriate buffer(s) (i. e. 
plan) from the world model and transmits them to the RTC, in which case RTC 
feedback goes to the TSCMP's indirectly. 

The world model is capable of storing plans and robot-environment 
states. The TSCMP's also include a permanently maintained data file for the 
storage of frequently accessed manipulator data (i. e. the "freeways" of 
Section IIIC 3). 
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Figure 20. Manipulator Software in Robot System 
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Plans developed in the TSC for the manipulator are coded for the RTC 
into one or more 2048 word 16-bit buffers. The buffers can be arranged into 
separate and complete groups, with sequencing within and between groups. 

Each individual buffer is organized into three parts: a header, a control stack, 
and an argument list. The header contains data pertaining to the sequencing of 
buffers. The control stack contains the sequence of specific commands to be 
executed and, for each command, a pointer to a place in the argument list 
where parameters of the command are to be found. Samples of currently- 
implemented functions (i. e. control stack entries) are provided in Appendix 7. 

Commands from above (i. e. from REX or the operator) are of four types. 
Bookkeeping commands affect the permanent arm data file and serve to define, 
name, store, delete, and retrieve data. Status commands are requests for 
output either to the operator or to REX. Planning commands result in the 
filling of RTC-bound buffers; examples include moving the arm, opening and 
closing the hand, exerting a force, and moving a single joint, as well as other 
commands. The fourth command type, execution commands, affect the RTC, 
causing the RTC to read, clear, or execute buffers. 

As an example of manipulator software functioning, consider the 
following. An operator provides a high-level command to retrieve a green rock. 
The cognitive executive initiates first TV/laser rangefinger planning activity, 
then vehicle motion activity; after these two plans are executed and another TV/ 
laser plan is formulated and executed, the rock is located as being within 
reach of the arm. A "MOVE" command is given to the TSCMP's. Programs 
performing the computations described in the above sections then determine the 
joint variables required for the hand to grasp the rock, compute a five -cubic 
trajectory for each joint to bring the arm to that position, and compute five 
sets of coefficient matrices for the equations of motion. Possibly more than 
five cubics are required for some joints, if obstacles are present. The poly- 
nomial coefficents and coefficient matrices are packed into the argument list, 
and hand opening, trajectory, and hand closing entries are packed into the con- 
trol stack. The buffer is transmitted to the RTC where each cycle the poly- 
nomials and coefficient matrices are used to compute nominal torques for each 
joint. To these nominal torques are added correction terms, and then these 
resultant torques are converted into current for transmission to the motors 
of the arm. Upon completion of the task, summary feedback is returned to the 

TSCMP's and the cognitive executive or operator. 
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VI. CONCLUSIONS 


The relationships governing the planning and implementation of specific 
arm motions have been summarized. The incorporation of the resulting 
manipulator software into an effective robotic appendage will require condi- 
tional planning and perhaps use of the full sensory capabilities of the 
manipulator. 

Conditional planning is currently being considered for implementation 
with proximity sensors, light -emitting diodes capable of providing direct 
range information. Addition of proximity sensors (Ref. 10) will enable the 
links of the arm to maintain a minimum safe distance from sensed obstacles 
without planning program intervention. Arm -mounted proximity sensors can 
also supplement TV/laser range finder data about the external environment. 

The arm has internal position and rate sensors. In addition, force can 
be implicitly sensed by monitoring motor currents. Tactile and proximity 
sensing and the possible future addition of an adaptive hand provide further 
sensory capabilities. Thus the arm can be used as a science instrument, 
possibly in real-time interaction with other sensors, in addition to its use as an 
effector. For instance, rocks can be weighed, external dimensions estimated, 
approximate densities determined, and perhaps even crude rock identifications 
made . 

Manipulator software efforts are currently directed towards the 
development of RTC software (Ref. 11) and the unification of TSC, RTC, and 
arm into a single system. Subsequent efforts will be directed towards the 
implementation of conditional planning and its integration with the already 
implemented deterministic planner, after which the sensory capabilities of 
the manipulator and its associated hardware are to be explored and exploited. 
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APPENDIX 2 


Kinematic Solutions 

2a) Direct Solution 

Define the vector 

d 

Observing that 


r 3 C 1 S 2 ' r 2 S 1 
r 3 S 1 S 2 + r 2 C 1 
r 3 c 2 




it is seen that 



and that there is no solution if Eq. (1) does not yield an t [l4. 0, 111. 8]. 
The cosine of 6^ is then uniquely determined by 
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Thus there are two possible values ofG^, one each corresponding to 


— dt 


K 


+ d! 


- r. 


yielding one left arm value for 0^ (positive) and one right arm value (negative). 


If e. 


>17 5 deg, there is no physically achievable solution. 
Solving the system 


yields the solution for 


< r 3 s 2^ C 1 " S 1 d l 
(r 2 ) Cj + (r 3 s 2 ) gj = d 2 

given by 


= tan 


-1 


/ d 2 r 3 s 2 - d l 
\ d l r 3 s 2 +d 2 



( 3 ) 


Knowing 0^, and permits calculatio n of the base system - boom trans- 
formation Tq given by 



r 

s i 

c l c 2 

C 1 S 2 

r 3 c l s 2" r 2 s l 

3 

" c i 

S 1 c 2 

s l s 2 

r 3 s l s 2 +r 2 c l 

T = 
0 

0 

" s 2 

c 2 

r 3 c 2 +r 1 


0 

0 

0 

1 
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Considering 


and defining 


the following 


Thus 



as being of the form 


x 3 y 3 z 3 P 3 
0 ° ° 1 


z - 


z 3 x a 


z 3 X a 


are true: 


'4 = ^3 


X z * z. 


s r - z-> x a * z 

5 3 


- z x s • a 


" s l s 4 + C 1 C 2 C 4 


C i S j 4 S, C-)C , 

14 12 4 


~ S 2 C 4 


- Yi. ' z 


c 5 z 3 * a 


c 6 ' z * s 



(4) 


( 5 ) 


( 6 ) 
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Given these values yield one solution. 

0 = (0^, 0^, r^, 0^, 0^, 9^) satisfies T{9) = T, then so does 

_A 

9 = ( e i» 9 2* r 3 ’ 9 4 + Tr ' " 0 5’ 9 6 + 7r > » (7) 

giving two solutions for each of the two possible values of O^. 

2t>) Incremental Solution 

Given a joint variable vector 0, the arm transformation T = T{^) is 
defined. The problem is to find the vector 0, where 


e = e + 6e , 

yielding an arm transformation T differing from T by a small amount 

AT e T - T . 

For convenience, several simplifying definitions are introduced: 

A. = l! , (9.) 

i l-l ' i 

A t » tJ.j (9.) , 


r 

“0 

-1 

0 

o' 


"0 

0 

0 

O' 

' 


1 

0 

0 

0 

for i f 3 and 

0 

0 

0 

0 

for i = 3 


0 

0 

0 

0 


0 

0 

0 

1 



0 

0 

0 

0 


0 

0 

0 

0 

j 


and 

U\ = A . . . A. , Q. A. . . . A, 

i 1 l-l ii b 

all for i = 1 ... 6 . 
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Using the approximations 


sin (9. + 60.) ~ sin 0. + cos 6^ 66^, 
cos {(T + 60^) % cos 0^ - sin 0^ 60^ 
and 60^ 60^ ~ 0 , 


it is seen that 


A. = 


t! , (0.) 

l-l i 


= T ) , (0. + 60.) « A (0.) + Q. A. (0.) 69. 
i-1 ' i i i 1111 


= X + Q. 3T. 60. 

i ill 


or 


A. « (I + Q. 60.) ~K. 

i ' ill 


(l) 


By definition of T f 


Aj • • • A^ T 


Substituting from Eq. (l), 

Aj . . . A 6 + Ql A t . . . A 6 6 6l + Aj Q 2 A 2 . . . A 6 68 2 + . . . + . . . X 5 Q 6 A 6 = T 

or 

U 60 x + . . . + U 6 60 6 = T - T = AT (2) 

This matrix relation contains 16 equations in six unknowns; all quantities except 
the 60. are known. Thus, six independent elements must be selected. 
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Suppose we wish to consider the original hand system rotated first about 
its normal axis by o' and then "tilted 11 about the sliding axis by e . The new hand 
orientation vectors can then be transformed by the original arm transformation 
T back to the base system and changes in position of {Ax, Ay, Az) added. 

That is, 


T = APT R 


O) 


where 


AP 


1 0 0 Ax 
0 1 0 Ay 
0 0 1 Az 
0 0 0 1 


and 


c 

6 

0 


O' 


'1 

0 

0 

0 “ 


" 1 

0 

€ 

0" 

0 

1 

0 

0 


0 



0 


0 

1 

a 

0 

~ S € 

0 

c e 

0 


0 

- s n 

C G 

0 


-€ 

- cy. 

1 

0 

0 

0 

0 

1 _ 


0 

0 

0 

1 


0 

0 

0 

1_ 


Observe that the small rotations a and e are with respect to the hand system; 
had it been desired to implement rotations with respect to the base system, we 
would have T - APRT, Changes in position are with respect to the base system. 

Considering T as of the form 


n i 

s i 

a l 

X 

_ 







n 2 

s 2 

a 2 

y 



— 

_ 


n 3 

S 3 

a 3 

z 

0 

0 

0 

1 


52 


JPL Technical Memorandum 33-679 



the rotated and translated arm transformation T becomes, on substitution into 
Eq. (2), 


T = 


The expanded matrix in Eq 


-ca A 

-* a i 

a7 i + 

£n l 

Ax 

-ca 2 

- aI 2 

aI 2 + 

en 2 

Ay 

-,a 3 


aJ 3 + 

€n 3 

Az 

0 

0 

0 


0 

(4), 

then, 

is AT. 




+ T (4) 


Selecting for the six required independent equations those corresponding 
to the first three elements in the last (AP) column of AT, two from the third 
column (Aa), and one from the second column (As), Eq. (2) might look like 


'Uj (1,4) 

U 2 (l,4) 

... U 6 (l,4)' 


' 6e r 


' AT 14 _ 


"Ax 

Uj(2, 4) 

U 2 (2,4) 

... U 6 (2,4) 


66 2 


AT 24 


Ay 

U a (3,4) 

U 2 (3,4) 

... U 6 (3,4) 


6r 3 


AT 34 


Az 

Uj(3,3) 

U 2 (3,3) 

... U 6 (3,3) 


Cn 

CD 


AT 33 


*7 3 + €n 3 

U" A (2, 3) 

U 2 (2,3) 

... U 6 (2,3) 


60 5 


AT 23 


a7 2 + c7 2 

U 1 (3,2) 

U 2 (3,2) 

... U 6 (3,2) 


_ 60 6_ 


AT 32 

_ — 


_“ aI 3 


Therefore, as an example, to find the change 60 in the joint variable vector 0 
required to implement a small change in position (Ax, Ay, Az) with no change 
in orientation, Eq. (5) would be applied with a = e = 0. 
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APPENDIX 3 


Trajectory Specifications 


Define a normalized time 


u 



re 


[ T i-i, T i] 


running from 0 at the beginning of a trajectory segment i (when time t = t. 
to 1 at the end ( t = t ) for i = 1 . . . n. The trajectory is the sequence of poly- 
nomials h.(u) which together form 0 (t). If h = t\ - -j\ ^ is the time required 
for the i^ trajectory segment, then the following hold: 



po sition 

h.(u) 

= e(T) 


velocity 

V.(u) 

- 0 (T) 

and 

acceleration 

a i<u) 

= e(r) 



h.'(u)/t. 

h."(u)/t 2 
1 1 


Therefore 


hi (°) =' 6. ^ and h.(l) = 9., i = 1, n , 


and we define 


velocities 


v = v.(l) = h.(l)/t. , 

1 V 11 


the initial velocity 

the accelerations 

and the initial acceleration 


v 0 = v (0) , 


a. ~ a. ( 1 ) = h."(l)/t. , 

i i i i 


a Q s a^O) . 


As shown in Fig. 1, then. 


h.(u): (9 . v i , 1 . a ul ) 


( Q i» ai) 
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8(r ) = 8 
o o 

0(0 = v 
o o 

8(0 * o 
o o 


8 { t ] )= 8 j 


8 'Vl> - 


O * % ^ 


A PRIORI 
CONDITIONS 


«*■„> • 


°"J 


•> 

T 

O 






T n-I 


r_ 


#■ 



0 


I 0 



* * 



TRAJECTORY 

SEGMENT 

TIMES 

NORMALIZED 

TIME 


MO) = 0 

I o 

h l (°> - v o 

h, "(0) 


^(i) = h 2 «» = e } 


h n .,O) = h n (o)-0 n .i 


h : («) : (0, 


- r V I - 1' a j - V a i ) 


h n (1 >‘*n 

h n'(1) 

~ v " 
h „ "<’> 


Fig. 1. Trajectory Parameters 


Table 1 specifies the 14 constraints for (4, 3,4) and (3, 5,3) trajectories, 
and Tables 2 through 4 detail the trajectory polynomials for these two trajec- 
tories and one consisting of five cubic s. 
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Table 1. Conditions for the (4*3,4) and (3,5,3) trajectories 



Initial conditions 


Terminal conditions 


Intermediate conditions 


Continuous position 


Continuous velocity 


Continuous acceleration 
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Table 3. The (3,5,3) trajectory 
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Table 4, Five Cubic s 
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APPENDIX 4 


Relations Used in Obstacle Detection 


The minimum distance d Q between two lines 

p ^ 5 0 + . ( p - p 0 ) 


and 

where 


a, P 6 [0, 1 


Q - Q 0 + P (Qj - Q 0 > . 


and v =5 Pj - Pq 
is given by 


w = Qq - Q j 


u _ p 0 - Q a 


a + v + w 


where 


= <6, a, 1> , P Q = <0, p, l) 

[ b if b e [a , c] 

the relation <^a, b, = 


a if b < a 
c if b > c 


and ck - 


(u * w) (v • w) - (u • v) (w • w) 

-2 -2 ZZ —.2 

v w - (v ■ w) 

- (u • v)/ (v • v) 


if || v • w i || v 


otherwise 


(v ■ w) (u • v) - (v ♦ v) (u * w) 

~>2 - 2 —.2 
v w - (v • w) 


0 


if 1 1 v • w || 4 


otherwise 


The minimum distance between a point Qq = and a line P 

the same relations. Since Qq - Q , , then w - 0 and II v • w II = II \ 

A , A 1 

Thus a - - (u * v)/{v • v) and p = 0. 


I II w|| 

I II W II 

is given by 
Nl llwll = 0. 
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Link equations 
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APPENDIX 5 


Equations of Motion 


5a) General Derivation 

T 1 , i, j = 0, . . ., 6, is the transformation relating coordinate systems 
i and j, and the velocity V. in the base reference frame of a point 

x. 
i 

y i 


z. 

i 

1 


on link i is given by 


V i = ^( T 0 R i) 


or, alternatively, as 


R. = 
i 


V, 

l 





( 2 ) 


where q. are the components of the joint- variable vector. 


Note that V\ is expressed in system 0, the base frame, and in system i, 
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We will also have occasion to refer to the derivative 


3U. 


il 


referred to as U. and given by 
1 J K 


du. . 

U - LL 

ij k ~ 


tJ ■ 1 o T k " 1 O T 1 

i 0 j-1 U k 1 k-l 


1 Q, T ^' 1 Q T 1 
0 k k-1 j j - 1 


i 2k 2 j 
i 2 j 2k 
i < j or i < k 


( 6 ) 


Observe that U... = U., . 

ijk ikj , 


The expression of velocity in Eq. (2) then becomes 


V. = 
i 




(7) 


We now turn our attention to the total kinetic energy of (the six links of) 
the arm. The kinetic energy K of the system and K. of link i, i = 1, . . . , 6, 
are expressed in the base reference system. For link i, 


dK. 

i 


1 ( .2 

2 \ i 


2.2 
+ y i 



d 

m 



d 

m 
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Recalling 


that the inertia tensor I., can be expressed as 


I.. = 



if i = j 
if i 4 j 


where 



then can be expressed as 


J. = 

1 


-I + I + I 
xx yy z z 

Z 


I 

xy 


I 

xz 


m,x. 

l l 


I 

xy 


I 

xz 


1 * I + I 

xx yy z z j 

2 yz 


I + I - I 
xx yy zz 


m. y. 

i 7 1 


,( 10 ) 


m . z . 

l l 


m.x, 

l l 


m. y. 

l 7 1 


m. z. 
i i 


m. 

i 


and the kinetic energy of the arm as 


K 



i i 



Lp=l r = 1 


T 

U. J.U. 


ip i lr 


^pr 


(ID 
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Since matrix sum and trace operations commute and the scalar multi- 
plications can be equivalently performed on the trace, 


6 i i 



i= 1 j=l krl 



( 12 ) 


Observe that the J\ are dependent only upon the mass distribution of the 
links and not their position or rate of motion. Thus the J. need only be computed 
once. 

The potential energies P. of links i in the base frame sum to the total 
potential energy 

6 

p = £ p i 
i= 1 

where G is the gravity vector expressed in the base frame. For a level 
vehicle, G = (0, 0, -|g|, 0). 

Forming the Lagrangian L = K - P and then applying the Euler Lagrange 
equation 


V 

E 

i= 1 


- m.G 


( T o R 


i) 


(13) 


_ _d_ 9L cf L 

i _ dt 3q! 9q^ * 1 


6 , 


(14) 


for the generalized force (torque for all joints but the third) results in the 
equations of motion 


F. 

l 



j-i k=l 


Tr 


(U. J.U?) q 

' Jk j 


Tv + 


EEE 

j-i k= 1 m= 1 


Tr 


U.. J.U 

jkm j 


;>« 


q, T 
km 


6 

E m. GU..R, , 
J Ji J 

j=i 


i = 1, 


6 . 


(15) 
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where 


E v = E Tr ( u jk j j u J) 


ik Z-J iJ k 

j = max(i,k) j = max(i,k) 


C. 


u 

E 


ikm / j ^ijkm 

j = max(i, k, m) j = tnax(i, k, m) 


E Tr (wj u J) ■ 

/ ■ i \ r 


and 


C. = 
1 


6 


E 


D. . 
ij 



j-i 


(-m. GU.. R.). 
J Ji J 
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5b) Derivation of Equation (15)* 


The Lagrangian L = K - P is, from Eqs. (12) and (13), 

L = i E E Z Tr (vi u iU v* + E m i GT o ff i • 


i=i j=i k=i 


i= 1 


Before proceeding, we observe the following the facts: 


< Al) 4 : Tr (Vi u x T J = Tr \w n ( u ij J i u ik) 


= o , 


since the U's are dependent only on the joint variable and not its rate of change, 
and since J. is also independent of the rate of change of any joint variable. 


(A2 ) 




= 0 


(A3) 


3Tl 


= U. (from Eq. 5). 


(A4) 


D, .. = 
kl J 


Tr(u..J.U^) = Tr - Tr (u., TU?) = D 

\ ij i ik/ |_\ ij i ik/ J \ ik i ij / jik 


(J\ is symmetric). 


(A5 


dU.. 

_ ] 

dt 


E 9U. . \ 

-P 1 q = / U.. q 

oq ^m / a ljm m 


q - 
n m 

i m , 

m= 1 m= 1 


V U.. q 
/ a ijm m 


m= 1 


(from Eq. (6)), 


^Equation numbers refer to Appendix 5a. 
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(A9) D = -J- Tr/u.-J.uT.) = Tr (u.. J.U.'F ) + Tr (u., JU T | 

3q kij 8q \ ij i ik / \ ljp i ik / \ ikp i ij / 


6 i 


i=l j=l k=l 


6 3 T 1 

W p (D Kij ) 4 j q k + 2 m i G s i 


6 i i 


i~P j=l k=l 


Tr (u„ J.U* ) q.q 

\ ljp l ik / \ 


i-P j=l k=l 


6 

Tr(u.. J.U^lq.q. + V* m.GU. R. 

V ikp l lj / ^J H k £_J 1 ip 1 


With these observations in hand, the Euler Lagrange equations 


F _JL _ Q L 

p dt 96 3q 

P H P 


6 i 


6 i 


F = 
P 


D pik + 


i-P k=l 


i=p k= 1 m=l 


■( U ikm J i U ip ) ^k q , 


i=p k= 1 m= 1 


Tr (U. J.U.7 ) q, q 

\ ipm l ik I ^k b 


i=P j=l k=l 


Tr (U.. J.U^ ) q.q. 
\ UP 1 lk / 


72 


JPL Technical Memorandum 33-679 



6 i i 


.GU. R. . 

L ip 1 


i=p j=l k=l 


Since U.,, = U., . and dummy indices of summation. abound above, we 

ijk ikj 

can reindex and then note that the third, fourth, and fifth terms above cancel. 
Changing dummy indices, we have 


6 j 


F. = 
i 


( Vj°J ) ’) 


j = i k= 1 


6 J J 


EE S Tr ( U jkm J j U j9 Mm - Yj m j GU ji R j ’ 

j=i k= 1 m=l j = i 


as advertised. 
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APPENDIX 6 


Proofs of Coefficient Propositions 


Tr (u J.U 1 : \ . 

\ J J i / 

j = max |i, k| 

Since Tr(A) = Tr(A T ) and J T = J. , 

J J 

6 

o u- E Tt (Vi°j T 0 sc ki- 

j = max | i, k j 

Thus the coefficient for the reaction force on link i due to the acceleration 
of link k is equal to the reaction force coefficient for link k due to the 
acceleration of link i. This statement is in some sense analogous to a 
restatement of Newton’s third law. Thus C ^ is symmetric and contains only 
21 unique elements. 


Proposition 1 : = C^ 4 


Pf; 


C. 


ik 


E 


Proposition 2: C.. = C. 

c lkm imk 


Pf: 


C. 


ikm 


E Tr ( u Jkm j j u l) 

j= max | i, k, m| 


Since = ^jmk' statement i s proven. Clearly the effect on link i of 

the velocities of links k and m is the same as that of the velocities of links 
m and k. Of the 216 C.. , only 126 are unique. 
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Proposition 3 : - 0, m ^ 3. 

Pf: 


C. 


i3m 


S Tr ( T 0 °3 T 2 m_1 Q m T L-l J i T ?-l Q I T 0 lT ) 
j = max | m, i| 


Now 


Qs ^- 1 = Q 3 


0 0 0 0 
0 0 0 0 
0 0 0 1 
0 0 0 0 


since row 4 of T™ * = {0, 0, 0, 1). 

m- 1 

Furthermore, since Q-Q = 0 for all m, then so does Q^T-, Q » and so 

3 m. s m 

also does the whole matrix in parentheses above. Thus the trace and hence 

C (m > 3) = 0. Thus it is seen that the boom (link 3) velocity and the three 
i3m 

wrist link velocities do not have any interactive effect on any of the six joints. 
Of the 126 unique 24 are zero and thus only 102 need ever be calculated. 


Proposition 4 : C. km = - C mk . for i, m 2 k 


( Corollary 1 : 
Corollary 2: 


c iki = °- 


c kkk = 0 


> k 
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Pf: Assume without loss of generality that m > i. Then 


ikm 


b 

TrlxJ^Q T^Q XT'! T Q T Tj.' 1T ) 

/ j \ 0 kk-1 mtn-lji-l i 0 / 


j = m 


b 

T Tr (t^ _1 Q T^'/q T j J.TJ T Q 1 ^." 1 1 T ) 

L 4 \ o k k- 1 m m-1 j 1 - 1 i k - 1 0 / 


J = ni 


Since Tr(X) = TrfX 1 ) , 


C. 


ikrn 


E /^,k-l i-1 q T j j ,pjT qT m- 1 T_ T_,k- 1 T \ 

r \ 0 k-1 i i-1 j m-l U m T k -1 Q k T 0 / 


J- m 


Let 


A = 



a link transformation matrix, and 


B = 


T,' 1 Q T? J.T jT Q T T™', 1T 
k- 1 i i-I j m-1 m k-1 


Then 


C. 


ikrn 


E 


J=m 
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Since 


C mki 


= Yj Tr ( T ^ lQ k<'- 1 l Q i T i-i J j T ^-l Q m T r 1T ) 


J = m 


Y Tr ( AQ k BAT ) ■ 


j= m 


it need only be shown that 

Tr^AQ k BA T ) = -Tr^ABQ^ A T j , 

where A and B are defined above. In proposition 3, it was shown that 

C.» = 0 for m £ 3 and for arbitrary i. Thus C. 0 = - C -. = 0, so we need 

i3m 1.3 m m3i 

only prove the above relationship for k ^ 3. 

As a link transformation matrix, A is of the form 


n l 

s i 

a i 

Pi 

n 2 

S 2 

a 2 

P 2 

n 3 

s 3 

a 3 

p 3 

0 

0 

0 

1 


where n, s, and a are mutually orthogonal unit vectors. Utilization of this and 
straightforward multiplication of the matrices above gives us 


Tr (ABQ k T A T ) = b 21 - b J2 + b 4 ,(s • p) - b A , (n • p) 


42 


and 


Tr( A Q k BA T ) - -b 21 +bl 


I- b 1A (s • p) - b 24 (n * p) . 


2 14 
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It will be shown that = b^ 4 =0, j = 1, . . . , 4, thus proving that the 

traces are in fact equal in magnitude but opposite in sign and hence that 

C - -C . . (i, m > k), as claimed, 
lkm mki 

B is of the form 


T T 

B = TQXQ T , 


T 

where the T 1 s are link transformations. Thus row 4 of T {column 4 of T ) 

T 

is (0, 0, 0, 1). Also, row 4 of Q (column 4 of Q ) is (0, 0, 0, 0). So row 4 

T T 

of TQ and column 4 of Q T are therefore all 0, and hence so are the fourth 
row and column of B. QED. 


The link i force coefficient for the coupled velocities of links k and m 
is equal and opposite to the link m force coefficient of the coupled velocities 
of links k and i for k < m, i. Thus outer links' (i and m) velocities act reflex - 
ively to each other as they interact with the velocity of an inner link (k). 
Observe that though the coefficients are equal in magnitude, the force con- 
tributions are not, for in one case the force contribution is C.. q, q and in 

lkm^k m 

the other case the contribution is - Ck^^q^q.. The coefficient is multiplied 
by a different factor. 

Corollary 1 indicates that there is no interactive effect on any outer 
link (i) due to coupling of its own velocity with that of any inner link (k). 
Corollary 2 merely restates that the centrifugal force of a rotary joint is 
radial in direction and thus creates no torque felt by the motor. 

Only 56 of the C^. m need be computed. With the 21 C^ and six C^, 
only 83 C's are left. In fact, if the vehicle is gravitationally horizontal 
(i, e. , the gravity vector G = (0, 0, g, 0)), then there are only five gravity 
terms C^ to compute. . 


Proposition 5: C^ = 0 if G = (0, 0, g, 0). 

™ 6 6 

c i = =Z (- m j GQ i TJ 0 ^)- 

j=i j=i 

Since GQj = 0, - 0 also. 
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APPENDIX 7 


Implemented Control Stack Functions 


NUMBER 

NAME 

ACTION 

00 

TRAJ 

execute five cubic trajectory- 

01 

INCREM 

execute inc re mental -quintic trajectory 

02 

FORCE 

exert force or moment 

03 

RD POTS 

read pots 

04 

OPEN 

open hand 

05 

CLOSE 

close hand 

06 

RD DYN 

read dynamics (tachs, error torque, computed 
torque) after next command 

07 



10 

WAIT 

wait 

11 



12 



13 



14 



15 



16 



17 

FINI 

end -of-group 
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